#!/usr/bin/env python

import roslib; roslib.load_manifest('team4')
import rospy
from lift_test import Tray 
from geometry_msgs.msg import PointStamped
from ObjectUnstacker import *
from ObjectStacker import *
from PlatePicker import *
from Room3 import *
from free_table_spot import *
from pr2_python import base, head, gripper, world_interface
#from filename_of_your_module import * <- does not need the .py. Should import classes, functions, and maybe global vars

# Before calling import on your file, make sure of three things:
   # if it is team4/ then it can stay in scripts/ or be in src/
   # if it is in stacked_objects/ it has to be in src/
   # there cannot be an init_node in your code. If your code needs that to run, then you need an interface of some sort and we need to launch it seperately

# If there is anything that your code needs launched in addition to the usual, add it to the demo.launch file.

# Really, this should be done as a state machine. Just pretend it's one for now.


def stack_objects():
  tray = Tray()
  base_mover = base.Base()
  stacker = ObjectStacker()
  picker = PlatePicker()
  room3 = Room3()
  ou = ObjectUnstacker()

  gr_r = gripper.Gripper('right_arm')
  gr_r.open()
  gr_l = gripper.Gripper('left_arm')
  gr_l.open()

  print "Press key to move to stacking area"
  raw_input()

  rospy.loginfo("Stacking items on tray")
  # stacking objects code
  print "Press key to stack objects"
  raw_input()
  stacker.stack_objects()
  stacker.move_arms_back()

if __name__ == "__main__":
  rospy.init_node("team4_demo")

  rospy.loginfo("Beginning team 4 demo")
  rospy.loginfo("Creating all class instances")
  # instantiate classes here (if they need instantiating)

  stack_objects()
